/*
 *  particle.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 15/11/07.
 *
 */

#include "particle.h"

Particle::Particle()
:	Component(),
	mass(1.0)
{}

// ***

Particle::Particle(double cmass, vector<string> vp,int di) 
:	Component(vp,di),
	mass(cmass)
{}

void Particle::calcFi(const vector<double>& qddot)
{
	int i;

//	cout << "mass = " << mass  << endl;
	
	if(integratorType==0) for(i=0; i<dim; i++) Fi[i] = mass*qddot[ localGlob[i] ];
	else for(i=0; i<dim; i++) Fi[i] = mass*(dq[i]-dqC[i]);
	
//	for (i=0; i<dim; ++i)	cout << "particle Fi[" << i << "] = " << Fi[i] << endl;
}

// ***

void Particle::calcFq(const int& nt)
{}

// ***

void Particle::calcFt(const double& t, const double& grav, const int& gravDir)
{
/*
	if(Ft.size()!=0)
	{
		int i;
		for (i=0; i<dim; ++i)	cout << "particle Ft[" << i << "] = " << Ft[i] << endl;
	}
*/
}

// ***

void Particle::calcKi(double & partial_qddot)
{
	if(partial_qddot!=partial_qddotOld)	// In order to compute Ki just once
	{
		int i;	
		for(i=0; i<numDofs; i++) Ki(i,i) = mass*partial_qddot;
	}
	partial_qddotOld = partial_qddot;
}

// ***

void Particle::calcKq(int nt)
{}

// ***

void Particle::calcKdq(double & partial_qdot)
{}

// ***

void Particle::calcM(const vector<double>& q, const double& t)
{
	int i;
	if(M.size()==0)
	{	
		M.setZero(numDofs,numDofs);
		//for( i=0;i<numDofs;i++) M[i].resize(numDofs);
	}
	
	for(i=0; i<numDofs;i++) M(i,i) = mass;
}

// ***

void Particle::calcMLumped(const vector<double>& q, const double& t)
{
	if (MLumped.size()==0)
	{
		MLumped.resize(numDofs);
	}
	
	for (int i=0; i<numDofs; ++i)	MLumped[i] = M(i,i);	
	
}

// ***

void Particle::setDofs(void)
{
	map<string,Point*,less<string> >::iterator i;
	for(i=mPoints.begin(); i!=mPoints.end(); i++)	/** Loop on points belonging to this component */
	{
		for(int j=0; j<dim; j++)	(*i).second->setDof(0,j);	// 0 means only translational dof */
		for(int j=0; j<dim; j++)	(*i).second->setDofInertia(0,j);
	}
	numDofs = dim;
	numDofsInertia = dim;
}

// ***

void Particle::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{
	int i;
	int j;
	int p;
	
	// Initial position
	q.resize(numDofs);
	
	i=0;
	for(p=0; p<pointNames.size(); ++p)
	{
		for(j=0; j<dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
			q_in[ localGlob[i*dim+j] ] = mPoints[pointNames[p]]->r[j];
		}
		i++;
	}
	
	// Initial velocity
	// Default: initial velocity dofs are null
	dq.resize(numDofs);
	
	
/*
	int i=0;
	int j=0;
	
	time = t;
	if(q.size()==0)		{
		q.resize(numDofs);		
		for(i=0; i<pointNames.size(); i++)
		{
			for(j=0; j<dim; j++)
			{
				q_in[ localGlob[i*dim+j] ] = mPoints[pointNames[i]]->r[j] ;
			}
		}
	}
	else if(dq.size()==0)
	{
		dq.resize(numDofs);
		for(i=0; i<q.size(); i++)	dq_in[ localGlob[i] ] = 0.0 ;
	}
	else 
	{
		for(i=0; i<q.size(); i++)
		{
			dq_in[ localGlob[i] ] = dq[i] ;
		}
	}
*/
}
 
// ***

void Particle::setMatAtComp(Material* ma)
{}

// ***

void Particle::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool alg)
{
	if(grav!=0.0)
	{
		int i;
		int j;
		
		Ft.resize(numDofs);
		/*
		M.resize(numDofs);
		for(i=0; i<numDofs;++i)	M[i].resize(numDofs);
		makeM();	// Mass matrix can be defined once for all, since it is constant
		*/
		// Build Ft once for all  Ft=M*g
		/*
		for(i=0; i<numDofs; ++i)
		{
			Ft[i] = 0.0;
			for(j=0; j<pointNames.size();++j)	Ft[i]-=M[i][dim*j+gravDir-1]*grav;
		}
		*/
		Ft[gravDir-1] = -mass*grav;
	}
	
	if(st!=0)	Fi.resize(numDofs);
}

// ***

//void Particle::initializeCompTang(const bool& st,  const bool& pf)
void Particle::initializeCompTang(const bool& st)
{
	int i;
	if(st!=0)
	{
//		Ki.resize(numDofs);
//		for(i=0; i<numDofs;++i)	Ki[i].resize(numDofs);
		Ki.setZero(numDofs,numDofs);
		/*
		if(M.size()==0)	// Just in case M has not been created in initializeCompRes
		{	
			M.resize(numDofs);
			for(i=0; i<numDofs;++i)	M[i].resize(numDofs);
			makeM();	// Mass matrix can be defined once for all, since it is constant
		}
		*/
	}	
}
// ***
/*
void Particle::makeM()
{
	if(numDofs==3)	// dim=3
	{
		M[0][0] = mass;
		M[1][1] = mass; 
		M[2][2] = mass;
	} else if(numDofs==2)	// dim=2
	{
		M[0][0] = mass;
		M[1][1] = mass;
	} else	// dim=1
	{
		M[0][0] = mass;
	}		
}
*/
// ***

void Particle::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	int i;
	int j;
			
	// Kinetic
	if (dq.size()!=0 && (M.size()!=0) )
	{
		for (i=0; i<numDofs; ++i)	dq[i] = dq_in[localGlob[i]];
		for (i=0; i<numDofs; ++i)
		{
			for (j=0; j<numDofs; ++j)
			{
				Ekin+= 0.5*dq[i]*M(i,j)*dq[j];
			}
		}
	}
	// Potential
	for (i=0; i<numDofs; ++i)	q[i] = q_in[localGlob[i]];
	if( grav!=0.0) Epot+= mass*grav*q[gravDir-1];
}

// ***

void Particle::calcEntropy(double& Entropy, double& EntropyAmb)
{}

// ***

void Particle::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{
	if (numDofs>1)	// The angular momenta for a 1-D movement is null
	{
		int i;
		for (i=0; i<numDofs; ++i)
		{
			q[i] = q_in[localGlob[i]];
			dq[i] = dq_in[localGlob[i]];
		}
		am[2]+= mass*(q[0]*dq[1]- q[1]*dq[0]);
		if (numDofs>2)
		{
			am[0]+= mass*(q[1]*dq[2]- q[2]*dq[1]); 
			am[1]+= mass*(q[2]*dq[0]- q[0]*dq[2]); 
		}
	}
}	

// ***

void Particle::updateComp(void)
{}

// ***

void Particle::setIniDofs(vector<double>&)
{}

// ***

void Particle::calcPhi(const vector<double>&)
{}

// ***

void Particle::calcDPhi(const vector<double>&)
{}

// ***

void Particle::calcMSmooth(const vector<double>& q_in, const double& t)
{
	//	int i;
	//	if(MSmooth.size()==0)
	//	{
	//		MSmooth.resize(numDofsInertia);
	//		for( i=0;i<numDofsInertia;i++) MSmooth[i].resize(numDofsInertia);
	//	}
		MSmooth.setZero(numDofsInertia,numDofsInertia);

		MSmooth = MatrixXd::Identity(numDofsInertia,numDofsInertia);
	//	for(i=0; i<numDofsInertia;i++) MSmooth[i][i] = 1.0;
}

// ***

void Particle::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{
	FSmooth.setZero(2,numDofsInertia);
//	int i;
//	if (FSmooth.size()==0)		// Just in case M has not been created in initializeCompRes
//	{
//		FSmooth.resize(2);
//		for (i=0; i<2; ++i)	FSmooth[i].resize(numDofsInertia);
//	}
//	else setZero(FSmooth);
}

int Particle::checkState()
{
return 0;
}
